#ifndef PNP_H_
#define PNP_H_

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          vector<KeyPoint> &keypoints_1,
                          vector<KeyPoint> &keypoints_2,
                          vector<DMatch> &matches);

Point2d pixel2cam(const Point2d &p, const Mat &K);

void svd_icp(const vector<Point3f> &pts1,
             const vector<Point3f> &pts2,
             Mat &R, Mat &t);

void bundle_adjustment_g2o(const vector<Point3f> &pts1,
                           const vector<Point3f> &pts2,
                           Mat &R, Mat &t);

#endif
